#include "ros/ros.h"
#include "auto_msgs/localizition.h"
#include "auto_msgs/chassis.h"
#include "auto_msgs/control_cmd.h"
#include "auto_msgs/planning.h"
#include "auto_msgs/planning_point.h"
#include "common_tool/clampPid.h"
#include "common_tool/watchDog.h"
#include <memory>
#include <yaml-cpp/yaml.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <unistd.h>
#include <fcntl.h>
#include <thread>
#include <iostream>
#include "socket_can.hpp"
#include <yaml-cpp/yaml.h>
#include "dbc_file_analysis.h"
#include "dbc_canmsg_unpack.h"
#include "dbc_canmsg_pack.h"
#include "print_can_msg.h"
#include "canmsg_define.h"
#include "util.h"
#include "can_config_data_path.h"
watchDog sas_dog(8, 1, 0, 3, 8);
double steering_angle = 0;
void sas_cb(can_frame &frame)
{
	if (frame.can_id == 0x096)
	{

		//check sum
		uint8_T temp_data = 0;
		for (int i = 1; i < 8; i++)
		{
			temp_data ^= frame.data[i];
		}
		if (temp_data != frame.data[0])
		{
			std::cout << "check sum of sas is not right" << std::endl;
		}
		std::vector<double> sas_signals(8, 0);
		if (can_util::unpackCanmsg(dbc_analysis::DbcAnalysis::getInstance()
									   ->getMessages()[frame.can_id],
								   frame, sas_signals) == PACK_UNPACK_SUCCESS)
		{
			sas_dog.giveFood();
			if (sas_signals[2] == 0.0)
			{
				steering_angle = sas_signals[1];
			}
			else
			{
				steering_angle = -sas_signals[1];
			}
			std::cout << "sas steering_angle is:" << steering_angle << std::endl;
		}
	}
}
double rate = 100;
/*    watchDog(int foodPerTurn=0, int hungerPerTurn=0, int hungerMin=0,\
    int criticalPoint=50, int hungerMax=100); */

/*    clampPid( double Kp_local,double Ki_local,double Kd_local,\
              double deltaTime_local,\
              double downLimit_local,\
              double upLimit_local,\
              double IntergrationDownLimit_local,\
              double  IntergrationUplimit_local );*/
clampPid steer_angle_pid(1.0, 1.0, 0.0, 1.0 / rate, -10, 10, -10, 10);

int main(int argc, char **argv)
{
	//init dbc
	std::string folderpath = can_dbcs_path;
	std::vector<std::string> files;
	getAllFilesInFolder(folderpath, &files);
	for (std::string &file : files)
	{
		dbc_analysis::DbcAnalysis::getInstance()->addOneDbcFile(file);
	}
	dbc_analysis::DbcAnalysis::getInstance()->analysisFiles();
	dbc_analysis::DbcAnalysis::getInstance()->printMessages();
	//init socket_can
	socket_can::SocketCAN test_can("can0");
	test_can.register_asyn_read_cb(sas_cb);
	test_can.start_asyn_read();
	//get cmd value from yaml
	std::string file_name = can_config_data_path + "steer_test.yaml";
	YAML::Node steer_test_ymal = YAML::LoadFile(file_name); //也可以这样读取文件
	double angle_cmd = steer_test_ymal["angle_cmd"].as<double>();
	ros::init(argc, argv, "talker"); //初始化ros，向master注册一个叫“talker”的节点
	ros::NodeHandle n;				 //初始化一个句柄，就是将ROS实例化
	//消息队列相当于缓存消息，如果处理速度不够快的话消息会被保存在队列中
	ros::Rate loop_rate(rate); //执行循环的速率，可以理解为发布消息的频率，单位是Hz

	std::vector<double> eps_signals(10, 0);

	while (ros::ok()) //ros::ok()函数用来判断master节点是否正常
	{
		ros::spinOnce();
		/*if (chassis_dog.isDied())
		{
			std::cout << "chassis msg timeout" << std::endl;
			steer_angle_pid.resetIntergration();
			continue;
		}*/
		if (sas_dog.isDied())
		{
			std::cout << "can msg time out" << std::endl;
		}
		else
		{

			double angle_error_degree = (angle_cmd - steering_angle) * 3.14 / 180;
			std::cout << "###angle_cmd  is:" << angle_cmd << std::endl;
			std::cout << "###feed_back tire angle is:" << steering_angle << std::endl;
			std::cout << "angle error is:" << angle_error_degree << std::endl;
			double steer_torque = steer_angle_pid.step(angle_error_degree);
			std::cout << "steering torque is:" << steer_torque << std::endl;
			static double live_counter_130 = 0;
			if (live_counter_130 < 15)
			{
				live_counter_130 += 1;
			}
			else
			{
				live_counter_130 = 0;
			}
			eps_signals[0] = 0;	  //cheak sum
			eps_signals[1] = 0.0; //hand torque
			eps_signals[2] = 0;	  //error state
			eps_signals[3] = 0;	  //hand torque sign
			eps_signals[4] = 0;	  //0 hand of 1;1 hand on
			eps_signals[5] = 0;	  //hand off valid 0 ,invalid 1
			eps_signals[6] = 0.0; //LKA status,0x00,0x01(avaliable),0x02(active),0x03(fail)
			eps_signals[7] = live_counter_130;
			if (steer_torque < 0)
			{
				eps_signals[8] = -steer_torque;
				eps_signals[9] = 1;
			}
			else
			{
				eps_signals[8] = steer_torque;
				eps_signals[9] = -1;
			}
			//int packCanmsg(const Message &m, const std::vector<double> &signals_value, can_frame &msg);
			can_frame msg = {0};
			if (can_util::packCanmsg(dbc_analysis::DbcAnalysis::getInstance()->getMessages()[0x130], eps_signals, msg) != PACK_UNPACK_SUCCESS)
			{
				std::cout << "pack msg failed" << std::endl;
			}
			else
			{
				//cheak sum
				msg.data[0] = 0x00;
				for (int i = 1; i < 8; i++)
				{
					msg.data[0] ^= msg.data[i];
				}
				//test_can.write_frame(msg);
			}
		}
		sas_dog.noFood();
		loop_rate.sleep();
	}
	return 0;
}